#include "arch.h"
#include "KeypadTest.h"
#include "util.h"
#include "types.h"

volatile unsigned char scan_results[6] = {0x1f, 0x1f, 0x1f, 0x1f, 0x1f, 0x1f};
volatile unsigned char prev_results[6] = {0x1f, 0x1f, 0x1f, 0x1f, 0x1f, 0x1f};

void wls_keypad_init()
{
    return;
}

char wls_keypad_getkey()
{
    int row, col;
    int data;

    // Scan the keypad martix manually
    data = rGPBDAT | (0x3f << 5);
    for (col = 0; col < 6; col++) {
        rGPBDAT = data & (~(0x20 << col));
        ostdelay(100);
        scan_results[col] = rGPGDAT & 0x1f;
    }

    for (col = 0; col < 6; col++) {
        unsigned char key_changed = scan_results[col] ^ prev_results[col];
        if (!key_changed) continue;
        for (row = 0; row < 5; row++) {
            if (key_changed & (1 << row)) {
                // Just find one pressed or relase key
                // printk("row = %x, col = %x\n", row, col);
                memcpy((void *)prev_results, (void *)scan_results, sizeof(scan_results));
                return (row << 4) | col;
            }
        }
    }

    rGPBDAT &= ~(0x3f << 5);

    return 0xff;
}
